/**********************************************************************
 *   Copyright: (C)2024 LingYun IoT System Studio
 *      Author: LiJiahui<2199250859@qq.com>
 *
 * Description: The purpose of this code is to provide a simple C library,
 *              which providing software bit-bang of the I2C protocol on
 *              any GPIO pins for ISKBoard.
 *
 *   ChangeLog:
 *        Version    Date       Author            Description
 *        V1.0.0  2025.07.15    LiJiahui      Release initial version
 *
 ***********************************************************************/

#include <stdint.h>
#include <string.h>
#include <stdio.h>

#include "project_tasks.h"
#include "max30102.h"
#include "tim.h"
#include "i2c_bitbang.h"



// 静态变量，用于在不同函数调用之间保持状态
static uint32_t red_buffer[BUFFER_SIZE];
static uint32_t ir_buffer[BUFFER_SIZE];
static int32_t spo2 = 0, heart_rate = 0;
static int ir_smoothed = 0;
static int buffer_initialized = 0; // 标志位，确保初始化只执行一次



#define mdelay(x) HAL_Delay(x)

void max30102_reset(void)
{
    int                 rv=0;

    rv=max30102_write_reg(0x09, 0x40);  // 按字节写入寄存器
    HAL_Delay(10);  // 等待复位完成

    if(rv<0)
    {
    	printf("MAX30102 failed to reset!\n");
    }
    else
    {
    	printf("MAX30102 reset successfully!\n");
    }
}

int max30102_write_reg(uint8_t reg_addr, uint8_t value)
{
    uint8_t            buf[2] = {reg_addr, value};
    int                rv=0;

    i2c_lock(MAX30102_I2CBUS ,MAX30102_ADDR);          // 锁定设备
    rv=i2c_write(MAX30102_I2CBUS, buf, 2);                // 写入寄存器地址 + 数据

    if(rv<0)
    {
    	printf("MAX30102 failed to write!\n");
    	goto OUT;
    }

    i2c_term(MAX30102_I2CBUS);                       // 通信结束，释放总线
    return 0;
OUT:
	i2c_term(MAX30102_I2CBUS);
	return rv;

}

int max30102_read_reg(uint8_t reg_addr, uint8_t *buf, int size)
{
    int                rv = 0;
    int                i;
    uint8_t            byte;
    uint8_t            ack;

    i2c_lock(MAX30102_I2CBUS, MAX30102_ADDR);

    I2C_StartCondition(MAX30102_I2CBUS);

    // 发写地址
    if ((rv = I2C_SendAddress(MAX30102_I2CBUS, I2C_WR)) < 0)
        goto OUT;

    // 发寄存器地址
    if ((rv = I2C_WriteByte(MAX30102_I2CBUS, reg_addr)) < 0)
        goto OUT;

    // 重启信号
    I2C_StartCondition(MAX30102_I2CBUS);

    // 发读地址
    if ((rv = I2C_SendAddress(MAX30102_I2CBUS, I2C_RD)) < 0)
        goto OUT;

    for (i = 0; i < size; i++)
    {
        ack = (i < size - 1) ? ACK : NAK;
        if ((rv = I2C_ReadByte(MAX30102_I2CBUS, &byte, ack, I2C_CLK_STRETCH_TIMEOUT)) < 0)
            goto OUT;
        buf[i] = byte;
    }

OUT:
    I2C_StopCondition(MAX30102_I2CBUS);
    i2c_term(MAX30102_I2CBUS);

    if (rv < 0)
        printf("read_reg(0x%02X) failed! rv = %d\n", reg_addr, rv);

    return rv;
}


int maxim_max30102_init(void)
{
	int rv = 0;

	/* 配置中断使能寄存器 */
	if( (rv = max30102_write_reg(REG_INTR_ENABLE_1, 0xC0)) < 0 ) //0xC0:只使能FIFO满和数据就绪中断
		return false;
	if( (rv = max30102_write_reg(REG_INTR_ENABLE_2, 0x00)) < 0 ) //禁用温度就绪中断
		return false;

	/* 配置FIFO寄存器 */
	if( (rv = max30102_write_reg(REG_FIFO_WR_PTR, 0x00)) < 0 ) //重置FIFO写指针
		return false;
	if( (rv = max30102_write_reg(REG_OVF_COUNTER, 0x00)) < 0 ) //清零溢出计数器
		return false;
	if( (rv = max30102_write_reg(REG_FIFO_RD_PTR, 0x00)) < 0 ) //重置FIFO读指针
		return false;

	/* FIFO配置：样本平均数=8，禁用溢出回滚，FIFO满阈值=17 */
	if( (rv = max30102_write_reg(REG_FIFO_CONFIG, 0x0F)) < 0 )
		return false;

	/* 模式配置：SpO2模式（同时测量心率＋血氧） */
	if( (rv = max30102_write_reg(REG_MODE_CONFIG, 0x03)) < 0 )
		return false;

	/* SpO2配置：ADC量程=4096nA,采样率=400hz，LED脉冲宽度=411μs */
	if( (rv = max30102_write_reg(REG_SPO2_CONFIG, 0x27)) < 0 )
		return false;

	/* 配置LED驱动电流 */
	if( (rv = max30102_write_reg(REG_LED1_PA, 0x24)) < 0 ) //LED1(红光)电流~4.5mA
		return false;
	if( (rv = max30102_write_reg(REG_LED2_PA, 0x24)) < 0 ) //LED2(红外光)电流~4.5mA
		return false;
	if( (rv = max30102_write_reg(REG_SLOT1, 0x21)) < 0 ) //SLOT1=红，SLOT2=IR
		return false;
	if( (rv = max30102_write_reg(REG_SLOT2, 0x00)) < 0 ) //SLOT3=无，SLOT4=无
		return false;

	return true;

}

void MAX30102_Init(void)
{
    uint8_t                     dummy;

    i2c_lock(MAX30102_I2CBUS, MAX30102_ADDR);

    max30102_reset();
    HAL_Delay(10);

    do
    {
        max30102_read_reg(0x09, &dummy, 1);
    } while (dummy & 0x40);


    MAX30102_ClearInterrupt(); //清中断
    maxim_max30102_init(); //初始化MAX30102

    i2c_term(MAX30102_I2CBUS);
}

int MAX30102_Read_FIFO(uint32_t* red_led, uint32_t* ir_led)
{
    uint8_t             data[6] = {0};
    int                 rv = 0;

    rv = max30102_read_reg(REG_FIFO_DATA, data, 6);
    if (rv != 0)
    {
        printf("Failed to read MAX30102!\n");
        return rv;
    }

    *red_led = ((uint32_t)(data[0] & 0x03) << 16) | ((uint32_t)data[1] << 8) | data[2];
    *ir_led  = ((uint32_t)(data[3] & 0x03) << 16) | ((uint32_t)data[4] << 8) | data[5];

    return 0;
}

void MAX30102_ClearInterrupt(void)
{
    uint8_t               dummy;

    max30102_read_reg(0x00, &dummy, 1);  // INT_STATUS_1
    max30102_read_reg(0x01, &dummy, 1);  // INT_STATUS_2
}


/************************************************************************************
* *                     心率血氧饱和度计算算法部分
* **********************************************************************************/

const uint16_t auw_hamm[31]={ 41,    276,    512,    276,     41 }; //Hamm=  long16(512* hamming(5)');
//uch_spo2_table is computed as  -45.060*ratioAverage* ratioAverage + 30.354 *ratioAverage + 94.845 ; // 拟合公式
const uint8_t uch_spo2_table[184]={ 95, 95, 95, 96, 96, 96, 97, 97, 97, 97, 97, 98, 98, 98, 98, 98, 99, 99, 99, 99,
                            99, 99, 99, 99, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100,
                            100, 100, 100, 100, 99, 99, 99, 99, 99, 99, 99, 99, 98, 98, 98, 98, 98, 98, 97, 97,
                            97, 97, 96, 96, 96, 96, 95, 95, 95, 94, 94, 94, 93, 93, 93, 92, 92, 92, 91, 91,
                            90, 90, 89, 89, 89, 88, 88, 87, 87, 86, 86, 85, 85, 84, 84, 83, 82, 82, 81, 81,
                            80, 80, 79, 78, 78, 77, 76, 76, 75, 74, 74, 73, 72, 72, 71, 70, 69, 69, 68, 67,
                            66, 66, 65, 64, 63, 62, 62, 61, 60, 59, 58, 57, 56, 56, 55, 54, 53, 52, 51, 50,
                            49, 48, 47, 46, 45, 44, 43, 42, 41, 40, 39, 38, 37, 36, 35, 34, 33, 31, 30, 29,
                            28, 27, 26, 25, 23, 22, 21, 20, 19, 17, 16, 15, 14, 12, 11, 10, 9, 7, 6, 5,
                            3, 2, 1 } ;
static  int32_t an_dx[ BUFFER_SIZE-MA4_SIZE]; // delta
static  int32_t an_x[ BUFFER_SIZE]; //ir
static  int32_t an_y[ BUFFER_SIZE]; //red
void maxim_heart_rate_and_oxygen_saturation(uint32_t *pun_ir_buffer,  int32_t n_ir_buffer_length, uint32_t *pun_red_buffer, int32_t *pn_spo2, int8_t *pch_spo2_valid,
                              int32_t *pn_heart_rate, int8_t  *pch_hr_valid)
{
    uint32_t un_ir_mean ,un_only_once ;
    int32_t k ,n_i_ratio_count;
    int32_t i, s, m, n_exact_ir_valley_locs_count ,n_middle_idx;
    int32_t n_th1, n_npks,n_c_min;
    int32_t an_ir_valley_locs[15] ;
    int32_t an_exact_ir_valley_locs[15] ;
    int32_t an_dx_peak_locs[15] ;
    int32_t n_peak_interval_sum;

    int32_t n_y_ac, n_x_ac;
    int32_t n_spo2_calc;
    int32_t n_y_dc_max, n_x_dc_max;
    int32_t n_y_dc_max_idx, n_x_dc_max_idx;
    int32_t an_ratio[5],n_ratio_average;
    int32_t n_nume,  n_denom ;
    // remove DC of ir signal
    un_ir_mean =0;
    for (k=0 ; k<n_ir_buffer_length ; k++ ) un_ir_mean += pun_ir_buffer[k] ;
    un_ir_mean =un_ir_mean/n_ir_buffer_length ;
    for (k=0 ; k<n_ir_buffer_length ; k++ )  an_x[k] =  pun_ir_buffer[k] - un_ir_mean ;

    // 4 pt Moving Average
    for(k=0; k< BUFFER_SIZE-MA4_SIZE; k++)
    {
        n_denom= ( an_x[k]+an_x[k+1]+ an_x[k+2]+ an_x[k+3]);
        an_x[k]=  n_denom/(int32_t)4;
    }

    // get difference of smoothed IR signal

    for( k=0; k<BUFFER_SIZE-MA4_SIZE-1;  k++)
        an_dx[k]= (an_x[k+1]- an_x[k]);

    // 2-pt Moving Average to an_dx
    for(k=0; k< BUFFER_SIZE-MA4_SIZE-2; k++)
    {
        an_dx[k] =  ( an_dx[k]+an_dx[k+1])/2 ;
    }

    // hamming window
    // flip wave form so that we can detect valley with peak detector
    for ( i=0 ; i<BUFFER_SIZE-HAMMING_SIZE-MA4_SIZE-2 ;i++)
    {
        s= 0;
        for( k=i; k<i+ HAMMING_SIZE ;k++){
            s -= an_dx[k] *auw_hamm[k-i] ;
                     }
        an_dx[i]= s/ (int32_t)1146; // divide by sum of auw_hamm
    }


    n_th1=0; // threshold calculation
    for ( k=0 ; k<BUFFER_SIZE-HAMMING_SIZE ;k++)
    {
        n_th1 += ((an_dx[k]>0)? an_dx[k] : ((int32_t)0-an_dx[k])) ;
    }
    n_th1= n_th1/ ( BUFFER_SIZE-HAMMING_SIZE);
    // peak location is acutally index for sharpest location of raw signal since we flipped the signal
    maxim_find_peaks( an_dx_peak_locs, &n_npks, an_dx, BUFFER_SIZE-HAMMING_SIZE, n_th1, 30, 8 );//peak_height, peak_distance, max_num_peaks

    n_peak_interval_sum =0;
    if (n_npks>=2)
    {
        for (k=1; k<n_npks; k++)
            n_peak_interval_sum += (an_dx_peak_locs[k]-an_dx_peak_locs[k -1]);
        n_peak_interval_sum=n_peak_interval_sum/(n_npks-1);
        *pn_heart_rate=(int32_t)(6000/n_peak_interval_sum);// beats per minutes
        *pch_hr_valid  = 1;
    }
    else
    {
        *pn_heart_rate = -999;
        *pch_hr_valid  = 0;
    }

    for ( k=0 ; k<n_npks ;k++)
        an_ir_valley_locs[k]=an_dx_peak_locs[k]+HAMMING_SIZE/2;


    // raw value : RED(=y) and IR(=X)
    // we need to assess DC and AC value of ir and red PPG.
    for (k=0 ; k<n_ir_buffer_length ; k++ )
    {
        an_x[k] =  pun_ir_buffer[k] ;
        an_y[k] =  pun_red_buffer[k] ;
    }

    // find precise min near an_ir_valley_locs
    n_exact_ir_valley_locs_count =0;
    for(k=0 ; k<n_npks ;k++)
    {
        un_only_once =1;
        m=an_ir_valley_locs[k];
        n_c_min= 16777216;//2^24;
        if (m+5 <  BUFFER_SIZE-HAMMING_SIZE  && m-5 >0)
        {
            for(i= m-5;i<m+5; i++)
                if (an_x[i]<n_c_min)
                {
                    if (un_only_once >0)
                    {
                       un_only_once =0;
                   }
                   n_c_min= an_x[i] ;
                   an_exact_ir_valley_locs[k]=i;
                }
            if (un_only_once ==0)
                n_exact_ir_valley_locs_count ++ ;
        }
    }
    if (n_exact_ir_valley_locs_count <2 )
    {
       *pn_spo2 =  -999 ; // do not use SPO2 since signal ratio is out of range
       *pch_spo2_valid  = 0;
       return;
    }
    // 4 pt MA
    for(k=0; k< BUFFER_SIZE-MA4_SIZE; k++)
    {
        an_x[k]=( an_x[k]+an_x[k+1]+ an_x[k+2]+ an_x[k+3])/(int32_t)4;
        an_y[k]=( an_y[k]+an_y[k+1]+ an_y[k+2]+ an_y[k+3])/(int32_t)4;
    }

    //using an_exact_ir_valley_locs , find ir-red DC andir-red AC for SPO2 calibration ratio
    //finding AC/DC maximum of raw ir * red between two valley locations
    n_ratio_average =0;
    n_i_ratio_count =0;

    for(k=0; k< 5; k++) an_ratio[k]=0;
    for (k=0; k< n_exact_ir_valley_locs_count; k++)
    {
        if (an_exact_ir_valley_locs[k] > BUFFER_SIZE )
        {
            *pn_spo2 =  -999 ; // do not use SPO2 since valley loc is out of range
            *pch_spo2_valid  = 0;
            return;
        }
    }
    // find max between two valley locations
    // and use ratio betwen AC compoent of Ir & Red and DC compoent of Ir & Red for SPO2

    for (k=0; k< n_exact_ir_valley_locs_count-1; k++)
    {
        n_y_dc_max= -16777216 ;
        n_x_dc_max= - 16777216;
        if (an_exact_ir_valley_locs[k+1]-an_exact_ir_valley_locs[k] >10)
        {
            for (i=an_exact_ir_valley_locs[k]; i< an_exact_ir_valley_locs[k+1]; i++)
            {
                if (an_x[i]> n_x_dc_max) {n_x_dc_max =an_x[i];n_x_dc_max_idx =i; }
                if (an_y[i]> n_y_dc_max) {n_y_dc_max =an_y[i];n_y_dc_max_idx=i;}
            }
            n_y_ac= (an_y[an_exact_ir_valley_locs[k+1]] - an_y[an_exact_ir_valley_locs[k] ] )*(n_y_dc_max_idx -an_exact_ir_valley_locs[k]); //red
            n_y_ac=  an_y[an_exact_ir_valley_locs[k]] + n_y_ac/ (an_exact_ir_valley_locs[k+1] - an_exact_ir_valley_locs[k])  ;


            n_y_ac=  an_y[n_y_dc_max_idx] - n_y_ac;    // subracting linear DC compoenents from raw
            n_x_ac= (an_x[an_exact_ir_valley_locs[k+1]] - an_x[an_exact_ir_valley_locs[k] ] )*(n_x_dc_max_idx -an_exact_ir_valley_locs[k]); // ir
            n_x_ac=  an_x[an_exact_ir_valley_locs[k]] + n_x_ac/ (an_exact_ir_valley_locs[k+1] - an_exact_ir_valley_locs[k]);
            n_x_ac=  an_x[n_y_dc_max_idx] - n_x_ac;      // subracting linear DC compoenents from raw
            n_nume=( n_y_ac *n_x_dc_max)>>7 ; //prepare X100 to preserve floating value
            n_denom= ( n_x_ac *n_y_dc_max)>>7;
            if (n_denom>0  && n_i_ratio_count <5 &&  n_nume != 0)
            {
                an_ratio[n_i_ratio_count]= (n_nume*20)/n_denom ; //formular is ( n_y_ac *n_x_dc_max) / ( n_x_ac *n_y_dc_max) ;  ///*************************n_numeÔ­À´ÊÇ*100************************//
                n_i_ratio_count++;
            }
        }
    }

    maxim_sort_ascend(an_ratio, n_i_ratio_count);
    n_middle_idx= n_i_ratio_count/2;

    if (n_middle_idx >1)
        n_ratio_average =( an_ratio[n_middle_idx-1] +an_ratio[n_middle_idx])/2; // use median
    else
        n_ratio_average = an_ratio[n_middle_idx ];

    if( n_ratio_average>2 && n_ratio_average <184)
    {
        n_spo2_calc= uch_spo2_table[n_ratio_average] ;
        *pn_spo2 = n_spo2_calc ;
        *pch_spo2_valid  = 1;//  float_SPO2 =  -45.060*n_ratio_average* n_ratio_average/10000 + 30.354 *n_ratio_average/100 + 94.845 ;  // for comparison with table
    }
    else{
        *pn_spo2 =  -999 ; // do not use SPO2 since signal ratio is out of range
        *pch_spo2_valid  = 0;
    }
}

void maxim_find_peaks(int32_t *pn_locs, int32_t *pn_npks, int32_t *pn_x, int32_t n_size, int32_t n_min_height, int32_t n_min_distance, int32_t n_max_num)
{
    maxim_peaks_above_min_height( pn_locs, pn_npks, pn_x, n_size, n_min_height );
    maxim_remove_close_peaks( pn_locs, pn_npks, pn_x, n_min_distance );
    *pn_npks = min( *pn_npks, n_max_num );
}

void maxim_peaks_above_min_height(int32_t *pn_locs, int32_t *pn_npks, int32_t  *pn_x, int32_t n_size, int32_t n_min_height)
{
    int32_t i = 1, n_width;
    *pn_npks = 0;

    while (i < n_size-1)
    {
        if (pn_x[i] > n_min_height && pn_x[i] > pn_x[i-1])
        {            // find left edge of potential peaks
            n_width = 1;
            while (i+n_width < n_size && pn_x[i] == pn_x[i+n_width])    // find flat peaks
                n_width++;
            if (pn_x[i] > pn_x[i+n_width] && (*pn_npks) < 15 )
            {                            // find right edge of peaks
                pn_locs[(*pn_npks)++] = i;
                // for flat peaks, peak location is left edge
                i += n_width+1;
            }
            else
                i += n_width;
        }
        else
            i++;
    }
}


void maxim_remove_close_peaks(int32_t *pn_locs, int32_t *pn_npks, int32_t *pn_x, int32_t n_min_distance)
{

    int32_t i, j, n_old_npks, n_dist;

    /* Order peaks from large to small */
    maxim_sort_indices_descend( pn_x, pn_locs, *pn_npks );

    for ( i = -1; i < *pn_npks; i++ )
    {
        n_old_npks = *pn_npks;
        *pn_npks = i+1;
        for ( j = i+1; j < n_old_npks; j++ )
        {
            n_dist =  pn_locs[j] - ( i == -1 ? -1 : pn_locs[i] ); // lag-zero peak of autocorr is at index -1
            if ( n_dist > n_min_distance || n_dist < -n_min_distance )
                pn_locs[(*pn_npks)++] = pn_locs[j];
        }
    }

    // Resort indices longo ascending order
    maxim_sort_ascend( pn_locs, *pn_npks );
}

void maxim_sort_ascend(int32_t *pn_x,int32_t n_size)
{
    int32_t i, j, n_temp;
    for (i = 1; i < n_size; i++) {
        n_temp = pn_x[i];
        for (j = i; j > 0 && n_temp < pn_x[j-1]; j--)
            pn_x[j] = pn_x[j-1];
        pn_x[j] = n_temp;
    }
}

void maxim_sort_indices_descend(int32_t *pn_x, int32_t *pn_indx, int32_t n_size)
{
    int32_t i, j, n_temp;
    for (i = 1; i < n_size; i++)
    {
        n_temp = pn_indx[i];
        for (j = i; j > 0 && pn_x[n_temp] > pn_x[pn_indx[j-1]]; j--)
            pn_indx[j] = pn_indx[j-1];
        pn_indx[j] = n_temp;
    }
}


/* ----------------------------------- Functions used in FreeRTOS --------------------------- */


void max30102_init_buffer(void) {
    if (buffer_initialized) return;

    uint32_t red, ir;
    printf("Start to measure MAX30102!\r\n");

    MAX30102_Init();
    mdelay(100);

    for (int i = 0; i < BUFFER_SIZE;)
    {
        //while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_4) == GPIO_PIN_SET);
        MAX30102_Read_FIFO(&red, &ir);
        red_buffer[i] = red;
        ir_buffer[i] = ir;
        i++;
    }

    maxim_heart_rate_and_oxygen_saturation(ir_buffer, BUFFER_SIZE, red_buffer,
                                            &spo2, NULL, &heart_rate, NULL);
    buffer_initialized = 1;
}

/**
 * 传感器数据处理逻辑
 * 每次调用处理一帧数据
 */
SensorData_t max30102_process_data(void)
{
	uint32_t 	red, ir;
	int 		finger_present;
	int8_t 		spo2_valid, hr_valid;
	int 		ir_sum = 0;
	int 		ir_avg = 0;
	static int32_t 	heart_rate = 0, spo2 = 0;
	SensorData_t 	sensor_data;


    memmove(red_buffer, red_buffer + WINDOW_SHIFT,
            sizeof(uint32_t) * (BUFFER_SIZE - WINDOW_SHIFT));
    memmove(ir_buffer, ir_buffer + WINDOW_SHIFT,
            sizeof(uint32_t) * (BUFFER_SIZE - WINDOW_SHIFT));


    for (int i = BUFFER_SIZE - WINDOW_SHIFT; i < BUFFER_SIZE;)
    {
        //while (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_4) == GPIO_PIN_SET);
        MAX30102_ClearInterrupt();
        MAX30102_Read_FIFO(&red, &ir);
        red_buffer[i] = red;
        ir_buffer[i] = ir;
        i++;
    }

    ir_sum = 0;
    for (int i = 0; i < BUFFER_SIZE; i++)
        ir_sum += ir_buffer[i];
    ir_avg = ir_sum / BUFFER_SIZE;

    ir_smoothed = (int)(SMOOTHING_FACTOR * ir_avg + (1 - SMOOTHING_FACTOR) * ir_smoothed);

    finger_present = (ir_smoothed >= TOUCH_THRESHOLD) ? 1 : 0;

    if (!finger_present)
    {
        heart_rate = 0;
        spo2 = 0;
        hr_valid = 0;
        spo2_valid = 0;
    }
    else
    {
        maxim_heart_rate_and_oxygen_saturation(ir_buffer, BUFFER_SIZE,
                                               red_buffer, &spo2, &spo2_valid, &heart_rate, &hr_valid);

        if (!(hr_valid && spo2_valid && heart_rate <= 150 && spo2 <= 100))
        {
            heart_rate = 0;
            spo2 = 0;
        }
    }

    sensor_data.heart_rate = (uint8_t)heart_rate;
    sensor_data.spo2 = (uint8_t)spo2;

    printf("Analysis done, sent data to queue: HR=%d, SpO2=%d\r\n",
           sensor_data.heart_rate, sensor_data.spo2);

    return sensor_data;
}
